﻿// <copyright file="RigidBody.cs" company="Community Combat Simulator">
//    Community Combat Simulator: a free combat flight simulator.
//    Copyright (C) 2008 Aaron Schinkowitch
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see http://www.gnu.org/licenses/.
// </copyright>
namespace CCSim.Physics
{
    using System;
    using System.Globalization;
    using CCSim.Vecmath;

    /// <summary>
    /// A rigid body is a an object in space with position and orientation and which does not 
    /// deform.
    /// </summary>
    public class RigidBody
    {
        /// <summary>
        /// One divided by the mass of the rigid body in kilograms.
        /// </summary>
        private double inverseMass;
        
        /// <summary>
        /// The inverted inertia tensor of the rigid body.
        /// </summary>
        private Matrix inverseInertiaTensor;
        
        /// <summary>
        /// The rigid body's current state.
        /// </summary>
        private RigidBodyState currentState;
        
        /// <summary>
        /// The rigid body's calculated next state.
        /// </summary>
        private RigidBodyState nextState;

        /// <summary>
        /// Initializes a new instance of the RigidBody class with an initial mass and inverse 
        /// inertia tensor.
        /// </summary>
        /// <param name="mass">The body's mass in kilograms.</param>
        /// <param name="inverseInertiaTensor">The inverse inertia tensor.</param>
        public RigidBody(double mass, Matrix inverseInertiaTensor)
        {
            this.Mass = mass;
            this.InverseInertiaTensor = inverseInertiaTensor;
        }

        /// <summary>
        /// Gets a new vector containing the rigid body's position.
        /// </summary>
        /// <value>The current position of the rigid body's center of mass.</value>
        public Vector Position
        {
            get { return new Vector(this.currentState.Position); }
        }
        
        /// <summary>
        /// Gets the rigid body's mass in kilograms.
        /// </summary>
        internal double Mass 
        {
            get 
            { 
                return 1.0 / this.inverseMass; 
            }
            
            private set 
            { 
                if (value <= 0.0 || Double.IsNaN(value) || Double.IsInfinity(value))
                {
                    throw new ArgumentOutOfRangeException(
                        "value", "Mass must be positive; was " + value);
                } 
                
                this.inverseMass = 1.0 / value; 
            }
        }
        
        /// <summary>
        /// Gets the rigid body's inverse mass in 1/kg.
        /// </summary>
        internal double InverseMass 
        {
            get { return this.inverseMass; }
        }
                
        /// <summary>
        /// Gets or sets the rigid body's inverse inertia tensor, which defines how the body 
        /// responds to torques. The inverse inertia tensor must be invertible.
        /// </summary>
        internal Matrix InverseInertiaTensor 
        {
            get 
            { 
                return this.inverseInertiaTensor; 
            }
            
            set 
            {
                if (value == null) 
                {
                    throw new ArgumentNullException("value", "InverseInertiaTensor cannot be null");
                }
                
                if (!value.IsInvertible())
                {
                    throw new ArgumentOutOfRangeException(
                        "value", "InverseInertiaTensor must be invertable");
                }
                
                this.inverseInertiaTensor = value; 
            }
        }
        
        /// <summary>
        /// Gets the current state of the rigid body. It will be null if the rigid body has not been
        /// initialized.
        /// </summary>
        internal RigidBodyState CurrentState 
        {
            get { return new RigidBodyState(this.currentState); }
        }
        
        /// <summary>
        /// Gets the next state of the rigid body. The next state is the calculated next state of
        /// the rigid body that has not yet been applied to the rigid body.
        /// </summary>
        internal RigidBodyState NextState 
        {
            get 
            { 
                if (this.nextState == null)
                {
                    return null;
                }
                
                return new RigidBodyState(this.nextState); 
            }
        }
        
        /// <summary>
        /// Sets the initial conditions for a rigid body.
        /// </summary>
        /// <param name="currentTime">The starting time for the rigid body.</param>
        /// <param name="position">The initial position in world coordinates.</param>
        /// <param name="velocity">The initial velocity in m/s.</param>
        /// <param name="orientation">The initial orientation. Must be of unit length.</param>
        /// <param name="angularVelocity">The initial angular velocity in rad/s.</param>
        public void SetInitialConditions(
            double currentTime, 
            Vector position, 
            Vector velocity, 
            Quaternion orientation,
            Vector angularVelocity)
        {
            if (position == null)
            {
                throw new ArgumentNullException("position");
            }
            
            if (velocity == null)
            {
                throw new ArgumentNullException("velocity");
            }
            
            if (orientation == null)
            {
                throw new ArgumentNullException("orientation");
            }
            else if (!orientation.IsNormalized())
            {
                throw new ArgumentOutOfRangeException(
                    "orientation", "orientation must be normalized");
            }
            
            if (angularVelocity == null)
            {
                throw new ArgumentNullException("angularVelocity");
            }
            
            Vector angularMomementum = this.DeriveAngularMomentum(orientation, angularVelocity);
            
            this.currentState = new RigidBodyState(
                currentTime, position, velocity, orientation, angularMomementum);
        }
        
        /// <summary>
        /// Checks if the rigid body has been initialized. A rigid body is initialized by calling 
        /// SetInitialConditions.
        /// </summary>
        /// <returns>True if the rigid body has been initialized, false otherwise.</returns>
        public virtual bool IsInitialized()
        {
            return this.currentState != null;
        }
        
        /// <summary>
        /// Updates the next state of the rigid body given the current state.
        /// </summary>
        /// <param name="currentTime">The current time.</param>
        /// <param name="force">The sum of all forces acting on the body.</param>
        /// <param name="torque">The sum of all torques acting on the body.</param>
        public virtual void ComputeNextState(double currentTime, Vector force, Vector torque)
        {
            if (force == null) 
            {
                throw new ArgumentNullException("force");
            }
            
            if (torque == null)
            {
                throw new ArgumentNullException("torque");
            }
            
            if (this.currentState == null)
            {
                throw new InvalidOperationException("Initial conditions have not been set");
            }
            
            double elapsedTime = currentTime - this.currentState.Time;
            
            if (elapsedTime <= 0) 
            {
                throw new ArgumentOutOfRangeException(
                    "currentTime", 
                    currentTime + " not after current state's time: " + this.currentState.Time);
            }

            Vector position = new Vector(force).Scale(0.5 * elapsedTime * elapsedTime * InverseMass)
                .Add(this.currentState.Velocity * elapsedTime).Add(this.currentState.Position);
            
            Vector velocity = new Vector(force).Scale(elapsedTime * InverseMass)
                .Add(this.currentState.Velocity);
            
            // Calculate new Orientation
            Quaternion deltaOrientation 
                = (this.currentState.ComputeAngularVelocity(InverseInertiaTensor) 
                   * this.currentState.Orientation).Scale(0.5 * elapsedTime).Normalize();
            
            Quaternion orientation = deltaOrientation.MultiplyBy(this.currentState.Orientation);
            
            // New angular momentum
            Vector angularMomentum = new Vector(torque).Scale(elapsedTime)
                .Add(this.currentState.AngularMomentum);
            
            this.nextState 
                = new RigidBodyState(currentTime, position, velocity, orientation, angularMomentum);
        }
        
        /// <summary>
        /// Applies the computed next state to the rigid body, such that the next state becomes the
        /// current state for the rigid body, and the previous current state is discarded.
        /// </summary>
        public void UseNextState()
        {
            if (this.nextState == null) 
            {
                throw new InvalidOperationException("Next state is null. Call ComputeNextState first");
            }
            
            this.currentState = this.nextState;
            this.nextState = null;
        }
        
        /// <summary>
        /// Calculates the angular momentum of a rigid body given its orientation and angular 
        /// velocity.
        /// </summary>
        /// <param name="orientation">The rigid body's orientation.</param>
        /// <param name="angularVelocity">The rigid body's angular momentum.</param>
        /// <returns>The angular momentum.</returns>
        internal virtual Vector DeriveAngularMomentum(Quaternion orientation, Vector angularVelocity)
        {
            Matrix rotation = new Matrix().SetRotation(orientation);
            Matrix rotationTranspose = new Matrix(rotation).Transpose();
            
            Matrix inertiaTensor = new Matrix(InverseInertiaTensor).Invert();
            Matrix inertiaTensorLocal = rotation.MultiplyBy(inertiaTensor).MultiplyBy(rotationTranspose);
            
            return inertiaTensorLocal * angularVelocity;            
        }    

        /// <summary>
        /// Holds the state of a rigid body.
        /// </summary>
        internal class RigidBodyState
        {
            /// <summary>
            /// The time for the state.
            /// </summary>
            private double time;
            
            /// <summary>
            /// The position of the rigid body's center of mass.
            /// </summary>
            private Vector position;
            
            /// <summary>
            /// The rigid body's velocity vector.
            /// </summary>
            private Vector velocity;
            
            /// <summary>
            /// The rigid body's orientation.
            /// </summary>
            private Quaternion orientation;
            
            /// <summary>
            /// The rigid body's angular momentum.
            /// </summary>
            private Vector angularMomentum;

            /// <summary>
            /// Initializes a new instance of the RigidBodyState class providing full state 
            /// information.
            /// </summary>
            /// <param name="time">The point in time which the state represents.</param>
            /// <param name="position">The position of the rigid body's center of mass.</param>
            /// <param name="velocity">The rigid body's velocity.</param>
            /// <param name="orientation">The rigid body's orientation.</param>
            /// <param name="angularMomentum">The rigid body's angular momentum.</param>
            internal RigidBodyState(
                double time, 
                Vector position, 
                Vector velocity, 
                Quaternion orientation, 
                Vector angularMomentum)
            {
                this.time = time;
                this.position = position;
                this.velocity = velocity;
                this.orientation = orientation;
                this.angularMomentum = angularMomentum;                
            }
            
            /// <summary>
            /// Initializes a new instance of the RigidBodyState class copying the values from the 
            /// specified state.
            /// </summary>
            /// <param name="other">The state whose values should be copied.</param>
            internal RigidBodyState(RigidBodyState other) : this(
                other.time, 
                other.position, 
                other.velocity, 
                other.orientation, 
                other.angularMomentum) 
            {
            }
            
            /// <summary>
            /// Gets or sets the time for which the state is valid.
            /// </summary>
            internal double Time 
            {
                get { return this.time; }
                set { this.time = value; }
            }
            
            /// <summary>
            /// Gets or sets the position of the rigid body's center of mass.
            /// </summary>
            internal Vector Position 
            {
                get { return this.position; }
                set { this.position = value; }
            }
            
            /// <summary>
            /// Gets or sets the rigid body's velocity.
            /// </summary>
            internal Vector Velocity 
            {
                get { return this.velocity; }
                set { this.velocity = value; }
            }
            
            /// <summary>
            /// Gets or sets the rigid body's orientation.
            /// </summary>
            internal Quaternion Orientation 
            {
                get { return this.orientation; }
                set { this.orientation = value; }
            }
            
            /// <summary>
            /// Gets or sets the rigid body's angular momentum.
            /// </summary>
            internal Vector AngularMomentum 
            {
                get { return this.angularMomentum; }
                set { this.angularMomentum = value; }
            }

            /// <summary>
            /// Checks for equality with another state.
            /// </summary>
            /// <param name="obj">The other state.</param>
            /// <returns>true if the states are equal false otherwise.</returns>
            public override bool Equals(object obj)
            {
                RigidBodyState other = obj as RigidBodyState;
                
                return other != null
                    && this.time == other.time
                    && this.position.Equals(other.position)
                    && this.velocity.Equals(other.velocity)
                    && this.orientation.Equals(other.orientation)
                    && this.angularMomentum.Equals(other.angularMomentum);
            }
            
            /// <summary>
            /// Gets a hash code for the state.
            /// </summary>
            /// <returns>The hash code.</returns>
            public override int GetHashCode()
            {
                return 0;
            }
            
            /// <summary>
            /// Gets a string representation of the state.
            /// </summary>
            /// <returns>A string representation of the state.</returns>
            public override string ToString()
            {
                return GetType().Name + " {Position: " + this.position 
                    + " Velocity: " + this.velocity + " Orientation: " + this.orientation
                    + " Angular Momentum: " + this.angularMomentum + "}";
            }
            
            /// <summary>
            /// Comuputes the rigid body's angular momentum given its inverse inertia tensor.
            /// </summary>
            /// <param name="inverseInertiaTensor">The rigid body's inverse inertia tensor.</param>
            /// <returns>The rigid body's angular momentum.</returns>
            internal Vector ComputeAngularVelocity(Matrix inverseInertiaTensor)
            {
                if (inverseInertiaTensor == null) 
                {
                    throw new ArgumentNullException("inverseInertiaTensor");
                }
                
                Matrix rotation = new Matrix().SetRotation(this.Orientation);
                Matrix rotationTranspose = new Matrix(rotation).Transpose();
                Matrix localInverseInertiaTensor 
                    = rotation.MultiplyBy(inverseInertiaTensor).MultiplyBy(rotationTranspose);
                
                return localInverseInertiaTensor * this.AngularMomentum;
            }
        }           
    }
}
